function [ CMid ] = middcm(CPrev, CCurr) %#codegen
%MIDDCM 计算两个方向余弦矩阵的中间姿态矩阵
%
% Input Arguments:
% # CPrev, CCurr: 3*3矩阵，方向余弦矩阵
%
% Output Arguments:
% # CMid: 3*3矩阵，对应CPrev'*CCurr的一半旋转的中间姿态矩阵，如果CPrev=C_M1^F，CCurr=C_M^F，则CMid=C_M1/2^F
%
% References:
% # 《里程计组合导航算法》

DC0 = CPrev' * CCurr;
Dq0 = dcm2quat_cg(DC0); % cos(theta/2)+u*sin(theta/2)
q0 = sqrt((Dq0(1, 1)+1)/2); % cos(theta/4)
q1 = Dq0(1, 2)/q0/2; % i*sin(theta/4)
q2 = Dq0(1, 3)/q0/2; % j*sin(theta/4)
q3 = Dq0(1, 4)/q0/2; % k*sin(theta/4)
Dq1 = [ q0 q1 q2 q3 ];
Dq1 = quatnormalize_pgs(Dq1);
DC1 = quat2dcm_cg(Dq1);
CMid = CPrev * DC1;
end